#include "PID.h"


PID Yaw_PID = {
    .SetValue = 0.0f,
    .pid_Calc_out = 0.0f,
    .error = 0.0f,
    .error_last = 0.0f,
    .error_pre = 0.0f,
    .Kp = 6.5f, // 比例系数
    .Ki = 0.1f, // 积分系数
    .Kd = 2.0f, // 微分系数
    .integral = 0.0f,
    .integral_max = 30.0f // 积分最大值
};

//灰度传感器PID
PID GW_gray_PID = {
    .SetValue = 0.0f,
    .pid_Calc_out = 0.0f,
    .error = 0.0f,
    .error_last = 0.0f,
    .error_pre = 0.0f,
    .Kp = 30.0f, // 比例系数
    .Ki = 0.0f, // 积分系数
    .Kd = 0.0f, // 微分系数
    .integral = 0.0f,
    .integral_max = 100.0f // 积分最大值
};

PID Find_blobs_PID = {
    .SetValue = 0.0f,
    .pid_Calc_out = 0.0f,
    .error = 0.0f,
    .error_last = 0.0f,
    .error_pre = 0.0f,
    .Kp = 0.0f, // 比例系数
    .Ki = 0.0f, // 积分系数
    .Kd = 0.0f, // 微分系数
    .integral = 0.0f,
    .integral_max = 100.0f // 积分最大值
};


PID Go_Target_PID = {
    .SetValue = 0.0f,
    .pid_Calc_out = 0.0f,
    .error = 0.0f,
    .error_last = 0.0f,
    .error_pre = 0.0f,
    .Kp = 1.5f, // 比例系数
    .Ki = 0.1f, // 积分系数
    .Kd = 0.2f, // 微分系数
    .integral = 0.0f,
    .integral_max = 30.0f // 积分最大值
};


/*
位置式PID运算核心,输入参数pPID_Calc是类型为PID结构的结构体指针，Next_val是用户期望系统调节到输出此值然后稳定在这一状态的设定值，
返回值即为PID运算输出的结果。
*/
float PID_Calc(PID *pPID_Calc, float Next_val, float actual, uint8_t flag) // 位置式PID运算核心
{
    pPID_Calc->SetValue = Next_val;

    // 计算原始误差
    pPID_Calc->error = pPID_Calc->SetValue - actual;

    // 关键修改点：处理角度跳变问题（当flag=1时）
    if(flag == 1)
    {
        // 将误差映射到[-180, 180)区间
        if(pPID_Calc->error >= 180.0f)  pPID_Calc->error -= 360.0f;
        if(pPID_Calc->error <  -180.0f) pPID_Calc->error += 360.0f;
    }

    // --- 原有积分处理保持不变 ---
    float integral_max = pPID_Calc->integral_max; // 使用结构体中的积分最大值
    float integral_min = -pPID_Calc->integral_max; // 使用结构体中的积分最小值
    pPID_Calc->integral += pPID_Calc->error;

    if(pPID_Calc->integral > integral_max) {
        pPID_Calc->integral = integral_max;
    } else if(pPID_Calc->integral < integral_min) {
        pPID_Calc->integral = integral_min;
    }

    // --- 原有PID计算保持不变 ---
    pPID_Calc->pid_Calc_out = (pPID_Calc->Kp * pPID_Calc->error) 
                             + (pPID_Calc->Ki * pPID_Calc->integral)
                             + pPID_Calc->Kd * (pPID_Calc->error - pPID_Calc->error_last);

    pPID_Calc->error_last = pPID_Calc->error;
    return pPID_Calc->pid_Calc_out;
}

int PID_Calc_only_int(PID *pPID_Calc, float Next_val, float actual)
{
    pPID_Calc->SetValue = Next_val; // 把用户期望值赋给PID控制器的用户设定值

    pPID_Calc->error = pPID_Calc->SetValue - actual;

    // 积分项限幅，防止积分饱和
    float integral_max = 100.0f;  // 定义积分项的最大值，根据系统要求调整
    float integral_min = -100.0f; // 定义积分项的最小值，根据系统要求调整

    pPID_Calc->integral += pPID_Calc->error; // 累加积分

    // 限幅处理
    if (pPID_Calc->integral > integral_max)
    {
        pPID_Calc->integral = integral_max;
    }
    else if (pPID_Calc->integral < integral_min)
    {
        pPID_Calc->integral = integral_min;
    }

    // PID计算

    // 当前偏差作为下一次偏差的上次偏差保存
    pPID_Calc->error_last = pPID_Calc->error;

    // 四舍五入处理，确保输出的整数值对称
    int result = 0;
    if (pPID_Calc->pid_Calc_out < 0)
    {
        result = (int)(pPID_Calc->pid_Calc_out - 0.5f); // 负数四舍五入
    }
    else if (pPID_Calc->pid_Calc_out > 0)
    {
        result = pPID_Calc->pid_Calc_out; // 正数四舍五入
    }
    else
    {
        result = 0; // 0
    }

    // 返回 PID 运算结果
    return result;
}



float PID_encoder_calc(PID *pPID_Calc, float Next_val, float actual, uint8_t flag)
{
    pPID_Calc->SetValue = Next_val;

    // 计算原始误差
    pPID_Calc->error = pPID_Calc->SetValue - actual;

    // 关键修改点：处理角度跳变问题（当flag=1时）
    if(flag == 1)
    {
        // 将误差映射到[-180, 180)区间
        if(pPID_Calc->error >= 180.0f)  pPID_Calc->error -= 360.0f;
        if(pPID_Calc->error <  -180.0f) pPID_Calc->error += 360.0f;
    }

    // --- 原有积分处理保持不变 ---
    float integral_max = 30.0f;
    float integral_min = -30.0f;
    pPID_Calc->integral += pPID_Calc->error;

    if(pPID_Calc->integral > integral_max) {
        pPID_Calc->integral = integral_max;
    } else if(pPID_Calc->integral < integral_min) {
        pPID_Calc->integral = integral_min;
    }

    // --- 原有PID计算保持不变 ---
    pPID_Calc->pid_Calc_out = (pPID_Calc->Kp * pPID_Calc->error) 
                             + (pPID_Calc->Ki * pPID_Calc->integral)
                             + pPID_Calc->Kd * (pPID_Calc->error - pPID_Calc->error_last);

    pPID_Calc->error_last = pPID_Calc->error;
    return pPID_Calc->pid_Calc_out;
}
